
import cv2
import numpy as np
from PIL import Image
import matplotlib.pyplot as plt

def getStereoRectifyImageFromCameraParameters(P,imageSize):
    '''
    作者：SXL；
    日期：2022.9.5；
    功能：通过双目相机的内外参数和畸变系数进行立体校正，获得去畸变后的双目参数；
    输入：左右相机的内外参P、图像的分辨率imageSize;
    输出：左、右相机的映射矩阵，校正后左、右相机的映射矩阵;
    DEMO：
        import numpy as np
        import cv2
        import BinocularMeas as bl
        # 双目相机的内外参和畸变参数
        P= ( [[1.58194505e+03,6.11360774e+02,1.58103367e+03,4.81957409e+02,
            1.58550432e+03, 6.60125535e+02,1.58560729e+03 ,5.30058387e+02,
            0.99935972,0.00832822,-0.03479633,
            -0.00818233,0.99995714,0.00433311,
            0.03483093,-0.00404562,0.99938503,
            -1.45455737e+02,-1.00152336e-01,3.81513984e+00,
            -0.10048731,0.15942811,0.00067713,-0.00077331,0.02106416,
            -0.10669631,0.27109715,0.00093811,-0.00115607,-0.21457691]])
        # 图片分辨率
        imageSize=(1280,1024)
        P0,P1,Pl,Pr,mapLx,mapLy,mapRx,mapRy = getStereoRectifyImageFromCameraParameters(P, imageSize)
        print('左相机投影矩阵',P0)
        print('右相机投影矩阵',P1)
        print('校正后的左相机投影矩阵',Pl)
        print('校正后的右相机投影矩阵',Pr)
        print('左相机(x,y)映射',mapLx,mapLy)
        print('右相机(x,y)映射',mapRx,mapRy)
    '''
    # 左相机内参
    mtx_l = np.array([[P[0], 0, P[1], 0],
                      [0, P[2], P[3], 0],
                      [0, 0, 1, 0]])
    # 右相机内参
    mtx_r = np.array([[P[4], 0, P[5], 0],
                      [0, P[6], P[7], 0],
                      [0, 0, 1, 0]])
    # 右相机到左相机的旋转矩阵
    R_lr = np.array([[P[8], P[9], P[10]],
                     [P[11], P[12], P[13]],
                     [P[14], P[15], P[16]]])
    # 右相机到左相机的平移矩阵
    T_lr = np.array([[P[17]],
                     [P[18]],
                     [P[19]]])
    cameraMatrixL = mtx_l[:, 0:3]
    cameraMatrixR = mtx_r[:, 0:3]
    # 左相机畸变
    distCoeffL = np.array([P[20], P[21], P[22], P[23], P[24]])
    # 右相机畸变
    distCoeffR = np.array([P[25], P[26], P[27], P[28], P[29]])
    # 左相机到左相机的投影矩阵
    R_ll = ([[1, 0, 0],
             [0, 1, 0],
             [0, 0, 1]])
    T_ll = ([[0], [0], [0]])
    temp_R_ll = np.append(R_ll, T_ll, axis=1)
    _temp_R_ll = np.row_stack((temp_R_ll, [0, 0, 0, 1]))
    P0 = np.dot(mtx_l, _temp_R_ll)
    # 左相机到右相机的投影矩阵
    temp_R_lr = np.append(R_lr, T_lr, axis=1)
    _temp_R_lr = np.row_stack((temp_R_lr, [0, 0, 0, 1]))
    P1 = np.dot(mtx_r, _temp_R_lr)
    # # 立体校正
    Rl, Rr, Pl, Pr, Q, validROIL, validROIR = cv2.stereoRectify(cameraMatrixL, distCoeffL,
                                                                cameraMatrixR, distCoeffR,
                                                                imageSize, R_lr, T_lr,
                                                                flags=cv2.CALIB_ZERO_DISPARITY,
                                                                alpha=0, newImageSize=(0, 0))

    # 计算更正remap
    mapLx, mapLy = cv2.initUndistortRectifyMap(cameraMatrixL, distCoeffL, Rl, Pl, imageSize, cv2.CV_32FC1)
    mapRx, mapRy = cv2.initUndistortRectifyMap(cameraMatrixR, distCoeffR, Rr, Pr, imageSize, cv2.CV_32FC1)

    return P0,P1,Pl,Pr,mapLx,mapLy,mapRx,mapRy

def calCalibrationAccuracyFromCameraParameters(P,grayImageL,grayImageR):
    '''
    作者：SXL；
    日期：2022.9.5；
    功能：由双目相机的内外参和畸变参数对图像进行立体校正，对比原始和校正参数的坐标测量精度；
    输入：左右相机的内外参和畸变P、左右相机拍摄的棋盘格图片(grayImageL,grayImageR);
    输出：双目相机原始和校正内外参数识别的棋盘格相邻角点间距和基准距离的绝对误差;
    DEMO：
        import numpy as np
        import cv2
        from PIL import Image
        import matplotlib.pyplot as plt
        import BinocularMeas as bl
        # 双目相机的内外参和畸变参数
        P= ( [[1.58194505e+03,6.11360774e+02,1.58103367e+03,4.81957409e+02,
            1.58550432e+03, 6.60125535e+02,1.58560729e+03 ,5.30058387e+02,
            0.99935972,0.00832822,-0.03479633,
            -0.00818233,0.99995714,0.00433311,
            0.03483093,-0.00404562,0.99938503,
            -1.45455737e+02,-1.00152336e-01,3.81513984e+00,
            -0.10048731,0.15942811,0.00067713,-0.00077331,0.02106416,
            -0.10669631,0.27109715,0.00093811,-0.00115607,-0.21457691]])
        # 读取图片
        grayImageL = cv2.imread("L2.bmp", 0)
        grayImageR = cv2.imread("R2.bmp", 0)
        error=bl.calCalibrationAccuracyFromCameraParameters(P,grayImageL,grayImageR)
        print('测量棋盘格相邻角点间距和基准距离的绝对误差',error)
    '''
    # 图像的分辨率
    imageSize = (grayImageL.shape[1], grayImageL.shape[0])
    P0, P1, Pl, Pr, mapLx, mapLy, mapRx, mapRy = getStereoRectifyImageFromCameraParameters(P, imageSize)
    # 经过remap之后，左右相机的图像已经共面并且行对齐
    rectifyImageL = cv2.remap(grayImageL, mapLx, mapLy, cv2.INTER_LINEAR)
    rectifyImageR = cv2.remap(grayImageR, mapRx, mapRy, cv2.INTER_LINEAR)

    # 没有校正后的图，在已经极线对齐的图片上均匀画线
    im_L=Image.fromarray(grayImageL)
    im_R = Image.fromarray(grayImageR)
    width = im_L.size[0] * 2
    height = im_R.size[1]
    img_compare1 = Image.new('RGBA', (width, height))
    img_compare1.paste(im_L, box=(0, 0))
    img_compare1.paste(im_R, box=(1280, 0))
    for i in range(0, 20):
        lenth = 1280/20
        plt.axhline(y=i * lenth, color='r', linestyle='-',lw=1)
    plt.imshow(img_compare1)
    plt.title('Image')
    plt.show()

    # 校正后的图，在已经极线对齐的图片上均匀画线
    im_L=Image.fromarray(rectifyImageL)
    im_R = Image.fromarray(rectifyImageR)
    width = im_L.size[0] * 2
    height = im_R.size[1]
    img_compare = Image.new('RGBA', (width, height))
    img_compare.paste(im_L, box=(0, 0))
    img_compare.paste(im_R, box=(1280, 0))
    for i in range(0, 20):
        lenth = 1280/20
        plt.axhline(y=i * lenth, color='r', linestyle='-',lw=1)
    plt.imshow(img_compare)
    plt.title('rectifyImage')
    plt.show()

    criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 30, 0.001)
    # 找校正后的棋盘格角点
    retL, cornersL = cv2.findChessboardCorners(rectifyImageL, (15, 11), None)
    retR, cornersR = cv2.findChessboardCorners(rectifyImageR, (15, 11), None)
    # 精细化找角点
    cornersL = cv2.cornerSubPix(rectifyImageL, cornersL, (15, 11), (-1, -1), criteria)
    cornersR = cv2.cornerSubPix(rectifyImageR, cornersR, (15, 11), (-1, -1), criteria)

    # 找原始图像的棋盘格角点
    retLO, cornersLO = cv2.findChessboardCorners(grayImageL, (15, 11), None)
    retRO, cornersRO = cv2.findChessboardCorners(grayImageR, (15, 11), None)
    # 精细化找角点
    cornersLO = cv2.cornerSubPix(grayImageL, cornersLO, (15, 11), (-1, -1), criteria)
    cornersRO = cv2.cornerSubPix(grayImageR, cornersRO, (15, 11), (-1, -1), criteria)
    # 在左图像中画出角点
    for i in range(0, len(cornersLO)):
        cv2.circle(grayImageL, (int(cornersLO[i][0][0]), int(cornersLO[i][0][1])), 1, (0, 0, 255))

    # 计算棋盘格角点的坐标
    i = 0
    w_distance = []
    wO_distance = []
    for i in range(cornersL.shape[0]):
        if (i + 1) % 15 == 0:
            continue
        # 校正图像第i个角点的左右质心坐标
        l0 = np.array([cornersL[i][0][0], cornersL[i][0][1]])
        r0 = np.array([cornersR[i][0][0], cornersR[i][0][1]])
        # 校正图像第i个角点的三维坐标
        s0 = np.array(cv2.triangulatePoints(Pl, Pr, l0, r0)).T
        w0 = s0[0][:-1] / np.max(s0[0][-1])
        # 校正图像第i+1个角点的左右质心坐标
        l1 = np.array([cornersL[i + 1][0][0], cornersL[i + 1][0][1]])
        r1 = np.array([cornersR[i + 1][0][0], cornersR[i + 1][0][1]])
        # 校正图像第i+1个角点的三维坐标
        s1 = np.array(cv2.triangulatePoints(Pl, Pr, l1, r1)).T
        w1 = s1[0][:-1] / np.max(s1[0][-1])
        # 校正图像横向相邻角点的间距
        distance = np.sqrt((w0[0] - w1[0]) ** 2 + (w0[1] - w1[1]) ** 2 + (w0[2] - w1[2]) ** 2)
        w_distance.append(distance)

        # 原始图像第i个角点的左右质心坐标
        lO0 = np.array([cornersLO[i][0][0], cornersLO[i][0][1]])
        rO0 = np.array([cornersRO[i][0][0], cornersRO[i][0][1]])
        # 原始图像第i个角点的三维坐标
        sO0 = np.array(cv2.triangulatePoints(P0, P1, lO0, rO0)).T
        wO0 = sO0[0][:-1] / np.max(sO0[0][-1])
        # 原始图像第i+1个角点的左右质心坐标
        lO1 = np.array([cornersLO[i + 1][0][0], cornersLO[i + 1][0][1]])
        rO1 = np.array([cornersRO[i + 1][0][0], cornersRO[i + 1][0][1]])
        # 原始图像第i+1个角点的三维坐标
        sO1 = np.array(cv2.triangulatePoints(P0, P1, lO1, rO1)).T
        wO1 = sO1[0][:-1] / np.max(sO1[0][-1])
        # 原始图像横向相邻角点的间距
        distance = np.sqrt((wO0[0] - wO1[0]) ** 2 + (wO0[1] - wO1[1]) ** 2 + (wO0[2] - wO1[2]) ** 2)
        wO_distance.append(distance)

    h_distance = []
    hO_distance = []
    for i in range(0, 15):
        for j in range(i, len(cornersR), 15):
            if j >= 150:
                continue
            # 校正图像第j个角点的左右质心坐标
            l0 = np.array([cornersL[j][0][0], cornersL[j][0][1]])
            r0 = np.array([cornersR[j][0][0], cornersR[j][0][1]])
            # 校正图像第j个角点的三维坐标
            s0 = np.array(cv2.triangulatePoints(Pl, Pr, l0, r0)).T
            w0 = s0[0][:-1] / np.max(s0[0][-1])
            # 校正图像第j+15个角点的左右质心坐标
            l1 = np.array([cornersL[j + 15][0][0], cornersL[j + 15][0][1]])
            r1 = np.array([cornersR[j + 15][0][0], cornersR[j + 15][0][1]])
            # 校正图像第j+15个角点的三维坐标
            s1 = np.array(cv2.triangulatePoints(Pl, Pr, l1, r1)).T
            w1 = s1[0][:-1] / np.max(s1[0][-1])
            # 校正图像纵向相邻角点的间距
            distance = np.sqrt((w0[0] - w1[0]) ** 2 + (w0[1] - w1[1]) ** 2 + (w0[2] - w1[2]) ** 2)
            h_distance.append(distance)

            # 原始图像第j个角点的左右质心坐标
            lO0 = np.array([cornersLO[j][0][0], cornersLO[j][0][1]])
            rO0 = np.array([cornersRO[j][0][0], cornersRO[j][0][1]])
            # 原始图像第j个角点的三维坐标
            sO0 = np.array(cv2.triangulatePoints(P0, P1, lO0, rO0)).T
            wO0 = sO0[0][:-1] / np.max(sO0[0][-1])
            # 原始图像第j+15个角点的左右质心坐标
            lO1 = np.array([cornersLO[j + 15][0][0], cornersLO[j + 15][0][1]])
            rO1 = np.array([cornersRO[j + 15][0][0], cornersRO[j + 15][0][1]])
            # 原始图像第j+15个角点的三维坐标
            sO1 = np.array(cv2.triangulatePoints(P0, P1, lO1, rO1)).T
            wO1 = sO1[0][:-1] / np.max(sO1[0][-1])
            # 原始图像纵向相邻角点的间距
            distance = np.sqrt((wO0[0] - wO1[0]) ** 2 + (wO0[1] - wO1[1]) ** 2 + (wO0[2] - wO1[2]) ** 2)
            hO_distance.append(distance)

    # 校正图像棋盘格的相邻角点间距
    distance = np.append(w_distance, h_distance)
    # 原始图像棋盘格的相邻角点间距
    distanceO = np.append(wO_distance, hO_distance)
    # 校正图像棋盘格相邻角点间距和基准距离的绝对误差
    error = (max(distance) - 30)
    # 原始图像棋盘格相邻角点间距和基准距离的绝对误差
    errorO = (max(distanceO) - 30)

    # 显示原始和校正后计算的棋盘格角点间距
    plt.figure(1)
    plt.title('Error value of calculation (mm)')
    plt.xlabel("The number of feature points (pcx)")
    plt.ylabel("Corners distance (mm)")
    plt.plot(distance, color='red', label=['Measurement accuracy after rectification', "%.3f" % float(error)])
    plt.plot(distanceO, color='gray', label=['Measurement accuracy before rectification', "%.3f" % float(errorO)])
    plt.legend(loc='best')
    plt.show()

    return error

def calCoordinateFromSpotcentroid(P,imgL, imgR):
    '''
    作者：SXL；
    日期：2022.10.23；
    功能：通过双目拍摄得到校正后光斑图像，再由左右相机的质心坐标计算出光斑三维坐标；
    输入：双目相机的参数和相机拍摄光斑的左右图像;
    输出：光斑在世界坐标系下的三维坐标。
    DEMO：
        import numpy as np
        import cv2
        import BinocularMeas as bl
        # 双目相机的内外参和畸变参数
        P= ( [[1.58194505e+03,6.11360774e+02,1.58103367e+03,4.81957409e+02,
            1.58550432e+03, 6.60125535e+02,1.58560729e+03 ,5.30058387e+02,
            0.99935972,0.00832822,-0.03479633,
            -0.00818233,0.99995714,0.00433311,
            0.03483093,-0.00404562,0.99938503,
            -1.45455737e+02,-1.00152336e-01,3.81513984e+00,
            -0.10048731,0.15942811,0.00067713,-0.00077331,0.02106416,
            -0.10669631,0.27109715,0.00093811,-0.00115607,-0.21457691]])
        # 读取图片
        imgL = cv2.imread("L1.bmp", 0)
        imgR = cv2.imread("R1.bmp", 0)
        w=np.array(bl.calCoordinateFromSpotcentroid(P,imgL,imgR))
        print('光斑的三维坐标',w)
    '''
    # 图像的分辨率
    imageSize = (imgL.shape[1],imgL.shape[0])
    P0,P1,Pl,Pr,mapLx,mapLy,mapRx,mapRy = getStereoRectifyImageFromCameraParameters(P, imageSize)
    # 经过remap之后，左右相机的图像已经共面并且行对齐
    rectifyimgL = cv2.remap(imgL, mapLx, mapLy, cv2.INTER_LINEAR)
    rectifyimgR = cv2.remap(imgR, mapRx, mapRy, cv2.INTER_LINEAR)
    # 阈值分割
    ret, thr = cv2.threshold(rectifyimgL, 175, 255, cv2.THRESH_BINARY)
    # 找到左图像轮廓并画出来
    contoursL, hie = cv2.findContours(thr, cv2.RETR_TREE, cv2.CHAIN_APPROX_SIMPLE)
    cv2.drawContours(rectifyimgL, contours=contoursL, contourIdx=-1, color=[0, 0, 255], thickness=2)
    contoursL = [cnt for cnt in contoursL if cv2.contourArea(cnt) > 30]
    # 算左图像质心
    for index in range(len(contoursL)):
        ML = cv2.moments(contoursL[index])
        cxL = round(ML['m10'] / ML['m00'], 3)
        cyL = round(ML['m01'] / ML['m00'], 3)
        # 在左图像中显示质心坐标
        cv2.putText(rectifyimgL, "X:" + str(cxL) + "   " + "Y:" + str(cyL), (50, (index + 1) * 50), cv2.FONT_HERSHEY_SIMPLEX, 1,(255, 255, 255), 1)
        centerlistL =np.array([cxL, cyL])

    ret, thr = cv2.threshold(rectifyimgR, 175, 255, cv2.THRESH_BINARY)
    # 找到右图像光斑轮廓并画出来
    contoursR, hie = cv2.findContours(thr, cv2.RETR_TREE, cv2.CHAIN_APPROX_SIMPLE)
    cv2.drawContours(rectifyimgR, contours=contoursR, contourIdx=-1, color=[0, 0, 255], thickness=2)
    contoursR = [cnt for cnt in contoursR if cv2.contourArea(cnt) > 30]
    # 算右图像质心
    for index in range(len(contoursR)):
        MR = cv2.moments(contoursR[index])
        cxR = round(MR['m10'] / MR['m00'], 3)
        cyR = round(MR['m01'] / MR['m00'], 3)
        # 在右图像中显示质心坐标
        cv2.putText(rectifyimgR, "X:" + str(cxR) + "   " + "Y:" + str(cyR), (50, (index + 1) * 50), cv2.FONT_HERSHEY_SIMPLEX,1, (255, 255, 255), 1)
        centerlistR = np.array([cxR, cyR])

    sO0 = np.array(cv2.triangulatePoints(Pl, Pr, centerlistL, centerlistR)).T
    W = sO0[0][:-1] / np.max(sO0[0][-1])
    # 将左右图像拼接在一起显示
    htitch = np.hstack((rectifyimgL, rectifyimgR))
    image = cv2.resize(htitch, None, fx=0.5, fy=0.5, interpolation=cv2.INTER_LINEAR)
    cv2.imshow("image", image)
    cv2.waitKey(0)
    return W


















